// 传感器任务代码文件

#include "system_support.h"
#include "freeRTOS.h"
#include "adaption_imu.h"
#include "adaption_magnetometer.h"
#include "user_uav_datatype.h"
#include "system.h"

#define SENSOR_TASK_TAG	"SensorTask"

IMU_Typedef IMU;
MagnetometerTypedef Magnetometer;


uint64_t eventtime;	// #Debug
/**
 * @brief 处理传感器的数据更新
 * 
 * @param argument 
 */
void SensorTaskFunc(void *argument)
{
	QueueSetMemberHandle_t	queue;
	EventStructdef event;
	enum UserUAVDatatype user_uav_datatype;
	
	/* 创建需要发送信息类型的队列 */
	taskENTER_CRITICAL();
	if(System.UserQueues.UAV_SendDataQueue == NULL)
		System.UserQueues.UAV_SendDataQueue = xQueueCreate(6, sizeof(user_uav_datatype));
	taskEXIT_CRITICAL();
	
	/* 等待传感器初始化 */
	vTaskDelay(50);
	
	while(IMU_init(&IMU) < 0){
		elog_error(SENSOR_TASK_TAG, "IMU init fail!");
	};

	while(Magnetometer_Init(&Magnetometer) < 0){
		elog_error(SENSOR_TASK_TAG, "Magnetometer init fail!");
	};
	// while(LC307_Init() < 0){
	// 	elog_error(SENSOR_TASK_TAG, "Flow init fail!");
	// };
	// while(RangingSensor_Init(&RangingSensor) < 0){
	// 	elog_error(SENSOR_TASK_TAG, "Ranging sensor init fail!");
	// };
	/* 模块初始化 */
	
	
	for(;;)
	{
		/* 等待队列集，并处理相对应的队列事件 */
		queue = xQueueSelectFromSet(System.SensorEventsSet, 10);
		
		/* 判断是哪个队列 */
		if(queue == System.SystemEvent.IMU_Sensor_EXTIQueue){
			xQueueReceive(queue, &event, 0);
			
			eventtime = event.eventTime;
			if(IMU_UpdateData(&IMU) < 0)
				elog_warn(SENSOR_TASK_TAG, "IMU update fail!");
			else{
				xQueueOverwrite(System.UserQueues.ImuDataQueue,&IMU.imu_msg);
				elog_info(SENSOR_TASK_TAG, "IMU data overwrite");
			}
			
		}
		else if(queue == System.SystemEvent.Magnetometer_EXTIQueue){
			xQueueReceive(queue, &event, 0);
			
			// TODO 数据更新断了自动重连
			if(Magnetometer_UpdateData(&Magnetometer) < 0)
				elog_warn(SENSOR_TASK_TAG, "Magnetometer update fail!");
			else{
				xQueueOverwrite(System.UserQueues.MagDataQueue,&Magnetometer.mag_msg);
				elog_warn(SENSOR_TASK_TAG, "Magnetometer data overwrite");
			}
			
		}
		// else if(queue == System.SystemEvent.Flow_UartRxQueue){
		// 	xQueueReceive(queue, &event, 0);
			
		// 	if(LC307_Unpack(event.dataLen) < 0)
		// 		elog_warn(SENSOR_TASK_TAG, "Flow update fail!");
		// 	else{
		// 		user_uav_datatype = FlowData;
		// 		if(xQueueSend(System.UserQueues.UAV_SendDataQueue, &user_uav_datatype, 0) != pdPASS){
		// 			elog_warn(SENSOR_TASK_TAG, "uav queue full");
		// 		}
		// 	}
			
		// }
		// else if(queue == System.SystemEvent.VL53L1_TimerQueue){
		// 	xQueueReceive(queue, &event, 0);
			
		// 	if(RangingSensor_UpdateData(&RangingSensor) < 0)
		// 		elog_warn(SENSOR_TASK_TAG, "Ranging sensor update fail!");
		// 	else{
		// 		user_uav_datatype = RangeData;
		// 		if(xQueueSend(System.UserQueues.UAV_SendDataQueue, &user_uav_datatype, 0) != pdPASS){
		// 			elog_warn(SENSOR_TASK_TAG, "uav queue full");
		// 		}
		// 	}
		// }
		else if(queue == NULL){
			/* 等待队列集超时报错 */
			elog_error(SENSOR_TASK_TAG, "wait queue set timeout!");
		}
		
		/* 检测磁力计连接是否正常 */
		// if(Magnetometer_CheckConnect(&Magnetometer) != 0){
		// 	elog_warn(SENSOR_TASK_TAG, "Mag disconnect,restarting");
		// 	int8_t ret = Magnetometer_Restart(&Magnetometer);
		// 	if(ret != 0)
		// 		elog_error(SENSOR_TASK_TAG, "Mag restart fail");
		// }
	}
}

